#include "kvh_component.hpp"

ORO_CREATE_COMPONENT(KVH::OroReadKVH)

namespace KVH{
  
  OroReadKVH::OroReadKVH(std::string name) : TaskContext(name,PreOperational)
  {
    this->addPort("tfData",tfData_port).doc("KVH output data");
  }
  
  bool OroReadKVH::configureHook()
  {
    log(Debug) << "(OroReadKVH) ConfigureHook entered" << endlog();
    kvh.initSerial();
    log(Debug) << "(OroReadKVH) initSerial method ended" << endlog();
    data = new IMUdata(); 
    log(Debug) << "(OroReadKVH) made new IMUdata object" << endlog();
    kvh.decodeIMU(data);
    log(Debug) << "(OroReadKVH) decodeIMU method ended" << endlog();
	log(Debug) << "(OroReadKVH) IMU data - xEulerAngle: " << data->xEulerAngle << endlog();
    log(Debug) << "(OroReadKVH) IMU data - yEulerAngle: " << data->yEulerAngle << endlog();
    log(Debug) << "(OroReadKVH) IMU data - zEulerAngle: " << data->zEulerAngle << endlog();
    log(Debug) << "(OroReadKVH) IMU data - xAccelerometer: " << data->xAccelerometer << endlog();
    log(Debug) << "(OroReadKVH) IMU data - yAccelerometer: " << data->yAccelerometer << endlog();
    log(Debug) << "(OroReadKVH) IMU data - zAccelerometer: " << data->zAccelerometer << endlog();
    
    return true;	
  }
  
  bool OroReadKVH::startHook()
  {
    log(Debug) << "(OroReadKVH) StartHook entered" << endlog();        
    return true;	
  }
  
  void OroReadKVH::updateHook()
  {        	
    kvh.decodeIMU(data);
    log(Debug) << "(OroReadKVH) decodeIMU method ended" << endlog();
	log(Debug) << "(OroReadKVH) IMU data - xEulerAngle: " << data->xEulerAngle << endlog();
    log(Debug) << "(OroReadKVH) IMU data - yEulerAngle: " << data->yEulerAngle << endlog();
    log(Debug) << "(OroReadKVH) IMU data - zEulerAngle: " << data->zEulerAngle << endlog();
    log(Debug) << "(OroReadKVH) IMU data - xAccelerometer: " << data->xAccelerometer << endlog();
    log(Debug) << "(OroReadKVH) IMU data - yAccelerometer: " << data->yAccelerometer << endlog();
    log(Debug) << "(OroReadKVH) IMU data - zAccelerometer: " << data->zAccelerometer << endlog();
    
    rot = KDL::Rotation::EulerZYX(data->zEulerAngle,data->yEulerAngle,data->xEulerAngle);
    tfData.transforms.resize(1);
    rot.GetQuaternion(tfData.transforms[0].transform.rotation.x, tfData.transforms[0].transform.rotation.y, tfData.transforms[0].transform.rotation.z, tfData.transforms[0].transform.rotation.w);
    tfData_port.write(tfData);
  }
}
